#include <modbus/modbus.h>

#include <chrono>
#include <cstring>
#include <iomanip>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

namespace {

void printUsage(const char* prog) {
    std::cout << "Usage: " << prog << " <serial> <baud> <parity> <data_bits> <stop_bits> <slave_id> <address> [count]" << std::endl;
    std::cout << "Example: " << prog << " /dev/ttyUSB0 9600 N 8 1 1 100 4" << std::endl;
}

void printRegisterValues(const std::vector<uint16_t>& values) {
    for (size_t i = 0; i < values.size(); ++i) {
        std::cout << "  [" << std::setw(2) << i << "] = " << values[i] << std::endl;
    }
}

void printDiscreteValues(const std::vector<uint8_t>& values) {
    for (size_t i = 0; i < values.size(); ++i) {
        std::cout << "  [" << std::setw(2) << i << "] = " << (values[i] ? "ON" : "OFF") << std::endl;
    }
}

}  // namespace

int main(int argc, char* argv[]) {
    if (argc < 8) {
        printUsage(argv[0]);
        return 1;
    }

    std::string serialPort = argv[1];
    int baudRate = std::stoi(argv[2]);
    char parity = argv[3][0];
    int dataBits = std::stoi(argv[4]);
    int stopBits = std::stoi(argv[5]);
    int slaveId = std::stoi(argv[6]);
    int address = std::stoi(argv[7]);
    int count = (argc > 8) ? std::stoi(argv[8]) : 1;

    std::cout << "================ Modbus RTU Test ================" << std::endl;
    std::cout << " Serial Port  : " << serialPort << std::endl;
    std::cout << " Baud Rate    : " << baudRate << std::endl;
    std::cout << " Parity       : " << parity << std::endl;
    std::cout << " Data Bits    : " << dataBits << std::endl;
    std::cout << " Stop Bits    : " << stopBits << std::endl;
    std::cout << " Slave ID     : " << slaveId << std::endl;
    std::cout << " Start Addr   : " << address << std::endl;
    std::cout << " Register Cnt : " << count << std::endl;
    std::cout << "================================================" << std::endl;

    modbus_t* ctx = modbus_new_rtu(serialPort.c_str(), baudRate, parity, dataBits, stopBits);
    if (!ctx) {
        std::cerr << "Failed to create Modbus RTU context: " << modbus_strerror(errno) << std::endl;
        return 1;
    }

    modbus_set_slave(ctx, slaveId);
    // 默认超时 3 秒
    timeval timeout {3, 0};
    modbus_set_response_timeout(ctx, timeout.tv_sec, timeout.tv_usec);

    if (modbus_connect(ctx) == -1) {
        std::cerr << "Failed to connect: " << modbus_strerror(errno) << std::endl;
        modbus_free(ctx);
        return 1;
    }

    std::cout << "✅ Connected successfully" << std::endl;

    // Holding Registers (FC03)
    std::vector<uint16_t> holding(count);
    if (modbus_read_registers(ctx, address, count, holding.data()) != -1) {
        std::cout << "\n[Holding Registers]" << std::endl;
        printRegisterValues(holding);
    } else {
        std::cout << "\n[Holding Registers] Failed: " << modbus_strerror(errno) << std::endl;
    }

    // Input Registers (FC04)
    std::vector<uint16_t> input(count);
    if (modbus_read_input_registers(ctx, address, count, input.data()) != -1) {
        std::cout << "\n[Input Registers]" << std::endl;
        printRegisterValues(input);
    } else {
        std::cout << "\n[Input Registers] Failed: " << modbus_strerror(errno) << std::endl;
    }

    // Coils (FC01)
    std::vector<uint8_t> coils(count);
    if (modbus_read_bits(ctx, address, count, coils.data()) != -1) {
        std::cout << "\n[Coils]" << std::endl;
        printDiscreteValues(coils);
    } else {
        std::cout << "\n[Coils] Failed: " << modbus_strerror(errno) << std::endl;
    }

    // Discrete Inputs (FC02)
    std::vector<uint8_t> discretes(count);
    if (modbus_read_input_bits(ctx, address, count, discretes.data()) != -1) {
        std::cout << "\n[Discrete Inputs]" << std::endl;
        printDiscreteValues(discretes);
    } else {
        std::cout << "\n[Discrete Inputs] Failed: " << modbus_strerror(errno) << std::endl;
    }

    modbus_close(ctx);
    modbus_free(ctx);

    std::cout << "\nTest completed." << std::endl;
    return 0;
}
